package behavior;
import lejos.nxt.ColorLightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.proposal.DifferentialPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

public class BehaviorController {
	
	private Arbitrator arbitrator;
	private Behavior[] behaviorList;
	final float leftWheelDiameter=5000;
	final float rightWheelDiameter=5000;
	final float trackWidth=5000;
	
	public BehaviorController() {
		Motor leftMotor = Motor.B;
		Motor rightMotor = Motor.C;
		DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		ColorLightSensor colorLightSensor = new ColorLightSensor(SensorPort.S1, ColorLightSensor.TYPE_COLORFULL);
		UltrasonicSensor ultrasonicSensor = new UltrasonicSensor(SensorPort.S2);
		ultrasonicSensor.continuous();
		behaviorList = new Behavior[3];
		behaviorList[0] = new CorrectToParallel(10, 1, pilot, ultrasonicSensor);
		behaviorList[2]= new DriveForward(pilot);
		behaviorList[1]= new LookForLine(colorLightSensor, pilot);
		arbitrator = new Arbitrator(behaviorList);
		arbitrator.start();
	
	}

}
